#ifndef POLE_VISION_H
#define POLE_VISION_H

#include <stdio.h>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
//#include <opencv/highgui.h>
#include <cv_bridge/CvBridge.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/thread.hpp>

#include <time.h>

#include <nodelet/nodelet.h>

#include <vector>
#include <numeric>
#include <functional>
#include <iostream>
#include <iterator>
#include "ransac.h"
#include "Kalman.h"
#include <Eigen/Geometry>

#include <geometry_msgs/Pose2D.h>
#include "std_msgs/Float32MultiArray.h"


#define SMALL_320_240
//#define MID_640_480

#ifdef SMALL_320_240

 #define IMG_WIDTH 320

 #define IMG_HEIGHT 240

#endif



#ifdef MID_640_480

 #define IMG_WIDTH 640

 #define IMG_HEIGHT 480

#endif

#define PI 3.141592653589793238462643383279
#define DEG_TO_RAD(x) (x*0.0174532925f)
#define RAD_TO_DEG(x) (x*57.2957795f)



//#define CHECK_DURA
//#define NOMAL_REGRESSION
//#define WTLS
#define RANSAC_MODEL
//#define SOBEL_Y
//#define DRAW
//#define KALMAN
//#define TRACKBAR

//#define MEDIAN_WINDOW_SIZE 20

#define V_GAP 6 
#define H_SEARCH_GAP 15  //( plus 10 and -10 px)
#define RANSAC_ITERATION 200
#define RANSAC_THRESHOLD 0.5
#define RANSAC_NUM_REQUIRE 12
#define REGRESSION_THRESHOLD 0.1
#define LOOP_HZ 100//1//100

using namespace std;
using namespace cv;

int nThreshold1=2287;
int nThreshold2=2291;
int nAperture=5;

int nthreshold_hough=44;

//int nparam1_hough=33;
//int nparam2_hough=24;
int nparam1_hough=21;
int nparam2_hough=63;

int delay = 0, key=0, i=0;
char *window_name;
FILE* fd;
//    CvCapture *video = NULL;
IplImage  *frame = NULL;
IplImage  *grey  = NULL;
IplImage  *canny_edges = NULL;
IplImage *sobel_edges_dy = NULL;
CvCapture* capture = NULL;
IplImage* draw_img = NULL;
CvMemStorage* storage = NULL;
CvSeq* lines=NULL;
// start and end times
time_t app_start,start,end;

ros::Publisher  m_pose_pub;
ros::Publisher m_pole_dec_pub;
ros::Publisher m_pose_init_pub;

Mat draw;

Mat grad_x,grad_y,abs_grad_x,abs_grad_y,grad;

// fps calculated using number of frames / seconds
double fps;

//double initialized=0;
vector<float> vline_length;
vector< vector<unsigned char> > vvpixel_profile;
vector<Point2f> line0_candi,line1_candi;
// frame counter
long counter = 0;

// floating point seconds elapsed since start
double sec;

char text[50];	
CvFont font;

LinearModel model; 
CRansacLinear Ransac0,Ransac1;
// Model-Parameters: y = m * x + b
const double MAX_OBSERVATION_POINTS = 24.0;
const double OBSERVATION_STEPWIDTH  = 1.0;

const double focal_length = 0.003204; //% in metre
const double pole_diameter = 0.16;    //% in metre  
const double pixel_size = 0.000012;    //% in metre 
const double b = 1.0;
const double pu = 156.86817; // principal point in u
const double pv = 102.65061; // principal point in v

//const double m = ((double)(2<<15) -b)/ (double)MAX_OBSERVATION_POINTS;
const double m = 1.0;
vector<Point2d> vline0_candi,vline1_candi,vline0_inliers,vline1_inliers,vline0_outliers,vline1_outliers;
vector<unsigned char> vPixel_weight0,vPixel_weight1;

std_msgs::Float32MultiArray msg_pole_out;
std_msgs::Float32MultiArray msg_pole_init;

typedef struct
{
  CvPoint pt0;
  CvPoint pt1;
} line_t;

typedef struct
{
  float m;
  float c;
  float r2;
} return_t;

return_t reg_ret;

line_t pole_left,pole_right;
double m0,m1;
int line0_x,line1_x;

return_t out0,out1;
unsigned char bootstrapping=0;
geometry_msgs::Pose2D m_pole_pose;

boost::mutex frame_grab_mutex;
bool flag_frame=FALSE;
bool get_line0=0;
bool get_line1=0;
FILE* fd_profile;
ros::Time current_time;

#ifdef KALMAN
Kalman kalman;
#endif

vector<line_t> vline;
vector<int> vsearch_v0,vsearch_v1,vsearch_v0_pre,vsearch_v1_pre;
vector<int> vsearch_u0,vsearch_u1;
unsigned char initialized=0;
vector<int> uf0,reg_uf0,reg_v0,uf1,reg_uf1,reg_v1;

Vector3f boot_homline0,boot_homline1;


return_t wtls(vector<int> y,vector<int> x,vector<unsigned char> w);
return_t regression(vector<int> x,vector<int> y);
double getDistance_Pt_Line(CvPoint& P, CvPoint& A, CvPoint& B);
Vector3f homline(Vector2f A, Vector2f B);



#endif

